// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org 



#ifndef _OpenMeca_Geom_Coordinate_hpp
#define _OpenMeca_Geom_Coordinate_hpp

#include <cmath>
#include <cassert>
#include <typeinfo>
#include <iostream>

#include <boost/array.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>


#include "OpenMeca/Geom/SpaceDim.hpp"
#include "OpenMeca/Geom/CoordinateSystem.hpp"
#include "OpenMeca/Geom/Frame.hpp"

using OpenMeca::Geom::_3D;
using OpenMeca::Geom::SpaceDim;

namespace OpenMeca
{
  namespace Geom
  {
    
    template<SpaceDim N> class Frame;
    template<SpaceDim N> class Matrix;
    //
    // Geom coordinates Referenced in a reference frame.
    //

    template<SpaceDim N, typename coordSystem = Cartesian>  // TODO : genericite contrainte ...
    class Coordinate
    {
    public: 	// Data
      static const double epsilon;
      static std::string GetStrKey(){return std::string("Coordinate" + SpaceDimUtil<N>::GetStrKey());}
      static const char * componentLabel[N];

      friend class Matrix<N>;
      friend class Vector<N>;
      friend class Point<N>;
      friend class Quaternion<N>;
      
      //Coordinate operator
      template<SpaceDim M, typename coordSyst> friend bool operator==(const Coordinate<M, coordSyst> &, const Coordinate<M, coordSyst> &);    
      template<SpaceDim M, typename coordSyst> friend bool operator!=(const Coordinate<M, coordSyst> &, const Coordinate<M, coordSyst> &);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator+   (const Coordinate<M, coordSyst> &a, const Coordinate<M, coordSyst> &b);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator-   (const Coordinate<M, coordSyst> &a, const Coordinate<M, coordSyst> &b);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator-   (const Coordinate<M, coordSyst> &a);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator+   (const Coordinate<M, coordSyst> &a, double k);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator-   (const Coordinate<M, coordSyst> &a, double k);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator*   (const Coordinate<M, coordSyst> &a, double k);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator*   (double k, const Coordinate<M, coordSyst> &a);
      template<SpaceDim M, typename coordSyst> friend Coordinate<M, coordSyst> operator/   (const Coordinate<M, coordSyst> &a, double k);
      //Vector operator
      template<SpaceDim M> friend std::ostream& operator<< (std::ostream &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator+ (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator- (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator- (const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator* (const Vector<M> &, const double &);
      template<SpaceDim M> friend Vector<M> operator* (const double &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator/ (const Vector<M> &, const double &);
      template<SpaceDim M> friend double   operator* (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend Vector<M> operator* (const Vector<M> &, const typename Vector<M>::DoubleArray &);
      template<SpaceDim M> friend Vector<M> operator^ (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend bool     operator!= (const Vector<M> &, const Vector<M> &);
      template<SpaceDim M> friend bool     operator== (const Vector<M> &, const Vector<M> &);
      //Quaternion operator
      template<SpaceDim M> friend std::ostream & operator<< (std::ostream& o, const Quaternion<M>& q);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Quaternion<M> &, const Vector<M> &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Vector<M> &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Quaternion<M> &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Quaternion<M> &, const double &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const double &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator+  (const Quaternion<M> &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator-  (const Quaternion<M> &, const Quaternion<M> &);
      //Matrix operator
      friend Vector<_3D> operator* (const Matrix<_3D> &, const Vector<_3D> &);
      friend Matrix<_3D> operator* (const double &, const Matrix<_3D> &);
      friend Matrix<_3D> operator* (const Matrix<_3D> &, const double &);
      friend Matrix<_3D> operator* (const Matrix<_3D> &m, const Matrix<_3D> &);
      friend Matrix<_3D> operator/ (const Matrix<_3D> &, const double &);

    public:
      explicit Coordinate(boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      Coordinate(double x, double y, double z, boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      ~Coordinate();

      Coordinate(const Coordinate & source);
      Coordinate & operator=(const Coordinate<N, coordSystem> & source);

      typedef coordSystem myCoordSystem;

      const Frame<N> & GetFrame() const;

      double & operator[](int i);
      const double & operator[](int i) const;

      void operator+= (double k);
      void operator-= (double k);
      void operator*= (double k);
      void operator/= (double k);
      void operator+= (const Coordinate<N, coordSystem> &);
      void operator-= (const Coordinate<N, coordSystem> &);
      void Clear();
 
      std::ostream &  Write(std::ostream & out) const;
      std::ostream &  XmlWrite(std::ostream & out) const;

      const double* GetCoordinates() const {return c_;}
      unsigned int GetRank() {return GetFrame().GetRank();};

      boost::function<const Frame<N>& ()>& GetFrameFunctionAccess() {return getFrame_;}
      const boost::function<const Frame<N>& ()>& GetFrameFunctionAccess() const {return getFrame_;}
      

    private:  // Data
      boost::array<double,N> c_;
      boost::function<const Frame<N>& ()> getFrame_;

       //BOOST SERIALIZATION       
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive & ar, const unsigned int)
      {
      	ar & BOOST_SERIALIZATION_NVP(c_);
      }

    public: 	// Data
      const SpaceDim dimension;
    };
    
    template<> inline
    void
    Coordinate<_3D, Cartesian>::operator+= (const Coordinate<_3D, Cartesian>& c)
    {
      assert(GetFrame() == c.GetFrame());
      c_[0]+=c.c_[0];
      c_[1]+=c.c_[1];
      c_[2]+=c.c_[2];
    }

    template<> inline
    void
    Coordinate<_3D, Cartesian>::operator-= (const Coordinate<_3D, Cartesian>& c)
    {
      assert(GetFrame() == c.GetFrame());
      c_[0]-=c.c_[0];
      c_[1]-=c.c_[1];
      c_[2]-=c.c_[2];
    }

    template<SpaceDim N, typename coordSystem>
    bool operator==(const Coordinate<N, coordSystem> &, const Coordinate<N, coordSystem> &);
    
    template<SpaceDim N, typename coordSystem>
    bool operator!=(const Coordinate<N, coordSystem> &, const Coordinate<N, coordSystem> &);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator+   (const Coordinate<N, coordSystem> &a, const Coordinate<N, coordSystem> &b);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator-   (const Coordinate<N, coordSystem> &a, const Coordinate<N, coordSystem> &b);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator-   (const Coordinate<N, coordSystem> &a);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator+   (const Coordinate<N, coordSystem> &a, double k);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator-   (const Coordinate<N, coordSystem> &a, double k);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator*   (const Coordinate<N, coordSystem> &a, double k);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator*   (double k, const Coordinate<N, coordSystem> &a);

    template<SpaceDim N, typename coordSystem>
    Coordinate<N, coordSystem> operator/   (const Coordinate<N, coordSystem> &a, double k);

    //
    // Vorbidden non-specialized methods (sould be specialized)
    //

    template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem>::Coordinate(boost::function<const Frame<N>& ()> f)
      : getFrame_(f), dimension(N)
    {
      assert(!"Coordinate<N, coordSystem> generic constructour should not be called !!!");
    }

    //
    // Template methods
    //

    //Descructor
    template<SpaceDim N, typename coordSystem>
    inline Coordinate<N, coordSystem>::~Coordinate()
    {}
         
    
    // Conversions ...

    // Accessors ...

    template<SpaceDim N, typename coordSystem>
    inline const Frame<N> & 
    Coordinate<N, coordSystem>::GetFrame() const
    {
      return getFrame_();
    }

    template<SpaceDim N, typename coordSystem>
    inline double &
    Coordinate<N, coordSystem>::operator[](int i) 
    {
      assert(i>=0 && i<N);
      return c_[i];
    }

    template<SpaceDim N, typename coordSystem>
    inline const double &
    Coordinate<N, coordSystem>::operator[](int i) const
    {
      assert(i>=0 && i<N);
      return c_[i];
    }
    

    // External Operators ...

    template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem> 
    operator+(const Coordinate<N, coordSystem> &a, const Coordinate<N, coordSystem> &b)
    {
      Coordinate<N, coordSystem> copy(a);
      copy+=b;
      return copy;
    }

    template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem> 
    operator-(const Coordinate<N, coordSystem> &a, const Coordinate<N, coordSystem> &b)
    {
      Coordinate<N, coordSystem> copy(a);
      copy-=b;
      return copy;
    }

    template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem> 
    operator-(const Coordinate<N, coordSystem> &a)
    {
      return a*(-1.);
    }

    template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem> 
    operator+(const Coordinate<N, coordSystem> &c, double k)
    {
      Coordinate<N, coordSystem> copy(c);
      for(int i=0; i< N; ++i)
	{
	  copy.c_[i]+=k;
	}
      return copy;
    }

   template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem> 
    operator-(const Coordinate<N, coordSystem> &c, double k)
    {
      Coordinate<N, coordSystem> copy(c);
      for(int i=0; i< N; ++i)
	{
	  copy.c_[i]-=k;
	}
      return copy;
    }

    template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem> 
    operator*(const Coordinate<N, coordSystem> &c, double k)
    {
      Coordinate<N, coordSystem> copy(c);
      for(int i=0; i< N; ++i)
	{
	  copy.c_[i]*=k;
	}
      return copy;
    }

    template<SpaceDim N, typename coordSystem> inline
    Coordinate<N, coordSystem> 
    operator/(const Coordinate<N, coordSystem> &c, double k)
    {
      Coordinate<N, coordSystem> copy(c);
      //if (fabs(k) < Coordinate<N, coordSystem>::epsilon) 
      if (fabs(k) ==0.)
	{
	  std::cerr << "operator/ : dividing a coordinate by a null value " << k << std::endl;
	  // TODO : generate an exception !!!
	}
      else
	{
	  for(int i=0; i< N; ++i)
	    {
	      copy.c_[i]/=k;
	    }
	}
      return copy;
    }

    template<SpaceDim N, typename coordSystem> inline
    bool
    operator==(const Coordinate<N, coordSystem> & a, const Coordinate<N, coordSystem> & b)
    {
      assert(a.GetFrame() == b.GetFrame());
      assert(typeid(a.myCoordSystem) == typeid(b.myCoordSystem));
      bool res=true;
      for(int i=0; i<N; ++i)
	{
	  res = res & (a.c_[i]==b.c_[i]);
	}
      return res;
    }

    template<SpaceDim N, typename coordSystem> inline
    bool
    operator!=(const Coordinate<N, coordSystem> & a, const Coordinate<N, coordSystem> & b)
    {
      return(! a==b);
    }

    // Class Operators ...

    template<SpaceDim N, typename coordSystem> inline
    void
    Coordinate<N, coordSystem>::operator+= (double k)
    {
      for(int i=0; i<N; ++i)
	{
	  c_[i]+=k;
	}
    }

    template<SpaceDim N, typename coordSystem> inline
    void
    Coordinate<N, coordSystem>::operator-= (double k)
    {
      for(int i=0; i<N; ++i)
	{
	  c_[i]-=k;
	}
    }
	
    template<SpaceDim N, typename coordSystem> inline
    void
    Coordinate<N, coordSystem>::operator*= (double k)
    {
      for(int i=0; i<N; ++i)
	{
	  c_[i]*=k;
	}
    }
    
    template<SpaceDim N, typename coordSystem> inline
    void
    Coordinate<N, coordSystem>::Clear ()
    {
      for(int i=0; i<N; ++i)
	{
	  c_[i]=0.;
	}
    }
    
    template<SpaceDim N, typename coordSystem> inline
    void
    Coordinate<N, coordSystem>::operator/= (double k)
    {
      /*if (fabs(k)<Coordinate<N, coordSystem>::epsilon)  
	{
	  std::cerr << "Coordinate::operator/= : dividing by a null value (" << k << ")" << std::endl;
	  // TODO : generate an exception !!!
	  }*/
       for(int i=0; i<N; ++i)
	{
	  c_[i]/=k;
	}
    }

    template<SpaceDim N, typename coordSystem> inline
    void
    Coordinate<N, coordSystem>::operator+= (const Coordinate<N, coordSystem> & c)
    {
      assert(GetFrame() == c.GetFrame());
      for(int i=0; i<N; ++i)
	{
	  c_[i]+=c.c_[i];
	}
    }

    template<SpaceDim N, typename coordSystem> inline
    void
    Coordinate<N, coordSystem>::operator-= (const Coordinate<N, coordSystem> & c)
    {
      assert(GetFrame() == c.GetFrame());
      for(int i=0; i<N; ++i)
	{
	  c_[i]-=c.c_[i];
	}
    }

     // Other Methods ...

    template<SpaceDim N, typename coordSystem> inline
    std::ostream & 
    Coordinate<N, coordSystem>::Write(std::ostream & out) const
    {
      std::cout.setf(std::ios::scientific, std::ios::floatfield);
      assert(typeid(myCoordSystem) == typeid(Cartesian));

      for (int i=0; i<N-1 ; ++i)
	{
	  out << c_[i] << ", " ;
	}
      return out <<  c_[N-1];
    }

    template<SpaceDim N, typename coordSystem> inline
    std::ostream & 
    Coordinate<N, coordSystem>::XmlWrite(std::ostream & out) const
    {
      std::cout.setf(std::ios::scientific, std::ios::floatfield);
      assert(typeid(myCoordSystem) == typeid(Cartesian));
      out << "<" << GetStrKey() << " " ;
       
      for (int i=0; i<N-1 ; ++i)
	{
	  out << componentLabel[i] << "=\"" << c_[i] << "\", " ;
	}
      out << componentLabel[N-1] << "=\"" <<  c_[N-1] << "\" />";
      return out;
    }

    //////////////////////////////////////////////////////////////////////////////////////////
    // 	template specialisation : _1D
    //////////////////////////////////////////////////////////////////////////////////////////


    ///////////////////////////////////
    // template specialisation : _2D //
    ///////////////////////////////////


    ///////////////////////////////////
    // template specialisation : _3D //
    ///////////////////////////////////

    // Constructors ...

    template<> inline
    Coordinate<_3D, Cartesian>::Coordinate(boost::function<const Frame<_3D>& ()> f)
      :  getFrame_(f), dimension(_3D)
    {
      c_[0]=c_[1]=c_[2]=0.;
    }

    template<> inline
    Coordinate<_3D, Cartesian>::Coordinate(double x, double y, double z, boost::function<const Frame<_3D>& ()> f)
      : getFrame_(f), dimension(_3D)
    {
      c_[0]=x;
      c_[1]=y;
      c_[2]=z;
    }

    //
    // Only authorized const Coordinate<_3D> & c can be used (authorized combination of coordSystem
    // and SpaceDim)
    //

    template<> inline
    Coordinate<_3D, Cartesian>::Coordinate(const Coordinate<_3D, Cartesian> & c)
      : getFrame_(c.getFrame_), dimension(_3D)
    {
      c_[0]=c.c_[0]; c_[1]=c.c_[1]; c_[2]=c.c_[2];
    }

    //
    // In this version only coordinate of same "coordSystem" are canidate for operator=().
    //

    template<> inline
    Coordinate<_3D> &
    Coordinate<_3D, Cartesian>::operator=(const Coordinate<_3D, Cartesian> & c)
    {
      assert(GetFrame() == c.GetFrame());

      if (this == &c)
	return *this;

      c_[0]=c.c_[0]; c_[1]=c.c_[1]; c_[2]=c.c_[2];
      return *this;
    }

  } // namespace Geom
}


#endif

